#include "energy.h"
#include "serial.h"

int enermy_is_red, enermy_is_blue;
int need_shoot = 0;
int whereArea = 0;                                 //演示用
int Whether_move = 0;                               //判断炮口是否已经移到想要的位置

Mat frame, src;
Mat ROI;
/**********以下参数是用于圆心和装甲板中心筛选的************/
vector<vector<Point>> Fans_contours;                //原始轮廓
vector<Vec4i> Fans_hierarchy;                       //初始层级状况
vector<vector<Point>> Fans_contours_out1;           //筛选完的装甲板轮廓
vector<vector<Point>> Fans_contours_out2;           //筛选完的圆心R字轮廓
vector<vector<int>> Fans_hierarchy_distinguish;     //用于遍历轮廓层级，达到筛选的作用
vector<int> Fans_hierarchy_dis_ele;                 //用于筛选装甲板轮廓的记录量
vector<double> Fans_contours_area;                  //用于提取圆心时的面积与找到的装甲板比值大小的记录
vector<double> Fans_contours_len_ar;                //用于提取圆心时的面积与周长的平方比值大小的记录

vector<Point3f> tempPointSet_board;                 //以装甲板中心为原点的三维坐标系
vector<Point3f> tempPointSet_circle;                //以圆心为原点的三为坐标系

Point2f point_board;                                //装甲板的中心点
Point2f point_circle;                               //圆心的中心点
Point2f point_board_clear;

vector<vector<Point>> ROI_Fans_contours;            //原始轮廓
vector<Vec4i> ROI_Fans_hierarchy;                   //原始层级状况
vector<vector<Point>> ROI_contours_out;             //筛选完的装甲板轮廓

//Point2f board_rect[4];                            //装甲板的四个角点
//vector<Point2f> board_rank_rect;                  //排完序的装甲板四个角点

//vector<Point3f> objpoint;                         //以圆心中心点为原点
//vector<Point3f> objpoint2;                        //物体的真实坐标，以毫米为单位，以装甲板中心点为原点

double theta;
double theta_judge=361;                             //判断角度是否发生突变，发生即是满足设中或者重新刷新条件

double shotPosition[8][2];                          //存储8个射击像素的xy位置
double kickAngle[8] = {45, 0, 315, 270, 225, 180, 135, 90 };                             //存储8个期望击中角度

Mat Energy::image_clear(Mat& image){
    //本函数的作用是图像处理，使图像易于后期运算处理
    vector<Mat> B_G_R;
    Mat src_thred;


    split(image, B_G_R);

    src_thred = B_G_R[2] - B_G_R[0];

    //图像二值化
    threshold(src_thred, src_thred, 45, 255, CV_THRESH_BINARY);
    Mat element = getStructuringElement(MORPH_RECT, Size(4, 4));
    Mat element2 = getStructuringElement(MORPH_CROSS, Size(3, 3));

    erode(src_thred, src_thred, element2);
    dilate(src_thred, src_thred, element);

    GaussianBlur(src_thred, src_thred, Size(3, 3), 0, 0);

    //imshow("二值化图像", src_thred);
    //Canny(src_thred, canny_result, 1, 3, 3);
    findContours(src_thred, Fans_contours, Fans_hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);

    return src_thred;

}

Mat Energy::Contours_out(Mat image, vector<vector<Point>> contours, vector<Vec4i> hierarchy){
    //本函数的作用是寻找装甲板的中心和能量机关的圆心
    /********************找到装甲板中心点*********************/

    //接下来进行筛选，筛选条件是有父轮廓，而无平级轮廓
    //cout << Fans_hierarchy_out.size()<<endl;

    Fans_hierarchy_distinguish.clear();
    Fans_hierarchy_dis_ele.clear();

    Fans_hierarchy_dis_ele.push_back(0);
    Fans_hierarchy_dis_ele.push_back(-1);
    //Fans_hierarchy_dis_ele中第一个元素自轮廓个数，第二个表示副轮廓对应的下标
    for (int i = 0; i < hierarchy.size(); i++)
    {
            Fans_hierarchy_distinguish.push_back(Fans_hierarchy_dis_ele);
    }

    for (size_t i = 0; i < hierarchy.size(); i++)
    {
            int count_father= hierarchy[i][3];
            if (count_father >= 0)
            {
                    Fans_hierarchy_distinguish[i][1] = count_father;
                    Fans_hierarchy_distinguish[count_father][0]++;
            }
    }


    int board_mark_target;
    int board_mark_correct_target = 0;//用于保证装甲板轮廓的单一性
    for (int i = 0; i < hierarchy.size(); i++)
    {
            int mark_father = Fans_hierarchy_distinguish[i][0];
            if (mark_father==1)
            {
                    for (size_t j = 0; j < hierarchy.size(); j++)
                    {
                            if (Fans_hierarchy_distinguish[j][1]==i)
                            {
                                    board_mark_target = j;
                                    board_mark_correct_target++;
                            }
                    }
            }
    }

    Fans_contours_out1.clear();

    if(board_mark_target == 1){
        Fans_contours_out1.push_back(contours[board_mark_target]);
        //drawContours(image, Fans_contours_out1, -1, Scalar(0,255,0), 1);

        //计算轮廓的矩
        vector<Moments> g_vMoments;
        g_vMoments.clear();
        g_vMoments.push_back(moments(Fans_contours_out1[0], true));

        //利用计算得到的 矩 计算中心矩
        vector<Point2f> centerMoments;
        centerMoments.clear();
        centerMoments.push_back(Point2f(float(g_vMoments[0].m10 / g_vMoments[0].m00), float(g_vMoments[0].m01 / g_vMoments[0].m00)));
        //将得到的中心矩显示出来
        point_board = (Point)centerMoments[0];
    }
    //else{
        //cout << "该帧不准确，跳过该帧" << endl;
    //}
    /********************找到圆心*********************/
    int center_mark_correct_target =0;//用于保证中心圆点的唯一性
    int center_mark_target;
    if(board_mark_target == 1){
        double Fans_board_area = contourArea(contours[board_mark_target]);
        for(int i = 0;i < contours.size(); i++){
            double area = contourArea(contours[i]);
            double length = arcLength(contours[i], true);
            Fans_contours_area.push_back(area / Fans_board_area);
            Fans_contours_len_ar.push_back((length * length) / area);
        }
        for(size_t i = 0; i < contours.size(); i++){
            if(Fans_contours_area[i] < circle_area1 && Fans_contours_area[i] > circle_area2 && Fans_contours_len_ar[i] < len_ar){
                center_mark_target = i;
                center_mark_correct_target++;
                Fans_contours_out2.push_back(contours[center_mark_target]);
            }

        }

        if(center_mark_target == 1){
            //drawContours(image, Fans_contours_out2, -1, Scalar(0, 255, 0), 1);
            //计算轮廓的矩
            vector<Moments> g_vMoments;
            g_vMoments.clear();
            g_vMoments.push_back(moments(Fans_contours_out2[0], true));
            //利用计算得到的 矩 计算中心矩
            vector<Point2f> centerMoments;
            centerMoments.clear();
            centerMoments.push_back(Point2f(float(g_vMoments[0].m10 / g_vMoments[0].m00), float(g_vMoments[0].m01 / g_vMoments[0].m00)));
            //将得到的中心矩显示出来
            point_circle = (Point)centerMoments[0];
            circle(image, point_circle, 3, Scalar(0, 0,255), -1, 8);
        }
        else{
            cout << "存在与中心原点极为相似的点，跳过该帧的圆心点" << endl;
        }
        //else {
            //cout << "存在与中心原点极为相似的点，同时暂停对圆心的识别"
       //}



    }
    return image;
}



Mat Energy::Theta_measurement(Mat image, vector<vector<Point>> Fans_contours_out1, vector<vector<Point>> Fans_contours_out2){

    if (Fans_contours_out1.size()!=0)
    {
        ROI = frame(Rect(point_board.x- frame.cols / partition_number, point_board.y - frame.rows / partition_number, frame.cols * 2 / partition_number, frame.rows * 2 / partition_number));
        Mat ROI_src = ROI;
        vector<Mat> ROI_B_G_R;
        Mat ROI_thred;
        split(ROI_src, ROI_B_G_R);

        ROI_thred = ROI_B_G_R[2] - ROI_B_G_R[0];

        //图像二值化
        threshold(ROI_thred, ROI_thred, 40, 255, CV_THRESH_BINARY);
        //imshow("ROI_thred",ROI_thred);
        findContours(ROI_thred, ROI_Fans_contours, ROI_Fans_hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
        //drawContours(ROI, ROI_Fans_contours, -1, Scalar(0, 255, 0), 1);
        int contours_number = 0;
        ROI_contours_out.clear();
        for (int i = 0; i < ROI_Fans_hierarchy.size(); i++)
        {
            int count_father = ROI_Fans_hierarchy[i][3];
            if (count_father>=0)
            {
                ROI_contours_out.push_back(ROI_Fans_contours[i]);
            }
        }

        if (ROI_contours_out.size() != 0)
        {
            vector< Point > hull;
            vector<vector<Point>> hull_select;
            hull_select.clear();
            for (int i = 0; i < ROI_contours_out.size(); ++i)
            {
                hull.clear();
                convexHull(ROI_contours_out[i], hull, false, true);
                hull_select.push_back(hull);
            }
            vector<float> area_select;
            area_select.clear();
            for (int i = 0; i < hull_select.size(); i++)
            {
                area_select.push_back(contourArea(hull_select[i]));
            }
            int mark = 0;
            float hull_s = area_select[0];
            for (int i = 0; i < area_select.size(); i++)
            {
                if (hull_s <= area_select[i])
                {
                    hull_s = area_select[i];
                    mark = i;
                }
            }
            if (hull_s > ROI.cols * ROI.rows / 10)
            {
                //计算轮廓的矩
                vector<Moments> g_vMoments;
                g_vMoments.clear();
                g_vMoments.push_back( moments(ROI_contours_out[mark], true));
                vector<Point2f> centerMoments;
                centerMoments.clear();
                centerMoments.push_back(Point2f(float(g_vMoments[0].m10 / g_vMoments[0].m00), float(g_vMoments[0].m01 / g_vMoments[0].m00)));
                point_board_clear = (Point)centerMoments[0];
                //circle(ROI, point_board_clear, 3, Scalar(0, 0, 255), -1, 8);
            }
            /*else
            {
                    cout << "没有找到和是的准确装甲板轮廓" << endl;
            }*/
            point_board_clear.x = point_board_clear.x + point_board.x - frame.cols / partition_number;
            point_board_clear.y = point_board_clear.y + point_board.y - frame.rows / partition_number;
            circle(image, point_board_clear, 3, Scalar(0, 0, 255), -1, 8);
            //现在统一规定，以x轴为基准，逆时针方向为正，角度0到360度，以此来分析小符的运动状态
            if (point_board_clear.x!= point_circle.x)
            {
                double k = abs((point_board_clear.y - point_circle.y) / (point_board_clear.x - point_circle.x));
                double theta_x = atan(k)*180/CV_PI;
                if (point_board_clear.y< point_circle.y&& point_board_clear.x>point_circle.x)
                {
                        theta = theta_x;
                }
                else if (point_board_clear.y< point_circle.y && point_board_clear.x<point_circle.x)
                {
                        theta = 180.0 - theta_x;
                }
                else if (point_board_clear.y > point_circle.y && point_board_clear.x < point_circle.x)
                {
                        theta = 180.0 + theta_x;
                }
                else if (point_board_clear.y > point_circle.y && point_board_clear.x > point_circle.x)
                {
                        theta = 360.0 - theta_x;
                }
                else if (point_board_clear.y = point_circle.y&& point_board_clear.x > point_circle.x)
                {
                        theta = 0.0;
                }
                else if (point_board_clear.y = point_circle.y && point_board_clear.x < point_circle.x)
                {
                        theta = 180.0;
                }
            }
            else
            {
                if (point_board_clear.y< point_circle.y)
                {
                        theta = 90.0;
                }
                else
                {
                        theta = 270.0 ;
                }
            }
                //cout << theta << endl;
        }
}
return image;



}


